﻿/*
  Numerical functions for computing minimizers of a least-squares system of equations.
  Copyright (C) 2011 Scott Schaefer

  This library is free software; you can redistribute it and/or
  modify it under the terms of the GNU Lesser General Public License
  (LGPL) as published by the Free Software Foundation; either
  version 2.1 of the License, or (at your option) any later version.

  This library is distributed in the hope that it will be useful,
  but WITHOUT ANY WARRANTY; without even the implied warranty of
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  Lesser General Public License for more details.

  You should have received a copy of the GNU Lesser General Public
  License along with this library; if not, write to the Free Software
  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
*/
using System;
using System.Collections.Generic;


namespace Mates
{
    public static class Eigen
    {
        public static float TOLERANCE = 0.0001f;
        static int method = 3;

        // for reducing two upper triangular systems of equations into 1
        public static void qr(float[] mat1, float[] mat2, float[] rvalue)
        {
            int i, j;
            float[,] temp1 = new float[8, 4];

            for (i = 0; i < 4; i++)
            {
                for (j = 0; j < i; j++)
                {
                    temp1[i, j] = 0;
                    temp1[i + 4, j] = 0;
                }
                for (j = i; j < 4; j++)
                {
                    temp1[i, j] = mat1[(7 * i - i * i) / 2 + j];
                    temp1[i + 4, j] = mat2[(7 * i - i * i) / 2 + j];
                }
            }

            qr(temp1, 8, rvalue);
        }

        // WARNING: destroys eqs in the process
        public static void qr(float[,] eqs, int num, float[] rvalue)
        {
            int i, j, k;

            qr(eqs, num, 0.000001f);
            for (i = 0; i < 10; i++)
            {
                rvalue[i] = 0;
            }

            k = 0;
            for (i = 0; i < num && i < 4; i++)
            {
                for (j = i; j < 4; j++)
                {
                    rvalue[k++] = eqs[i, j];
                }
            }
        }

        public static void qr(float[,] eqs, int num, float tol)
        {
            int i, j, k;
            float a, b, mag, temp;

            for (i = 0; i < 4 && i < num; i++)
            {
                for (j = i + 1; j < num; j++)
                {
                    a = eqs[i, i];
                    b = eqs[j, i];

                    if (Math.Abs(a) > 0.000001f || Math.Abs(b) > 0.000001f)
                    {
                        mag = (float)Math.Sqrt(a * a + b * b);
                        a /= mag;
                        b /= mag;

                        for (k = 0; k < 4; k++)
                        {
                            temp = a * eqs[i, k] + b * eqs[j, k];
                            eqs[j, k] = b * eqs[i, k] - a * eqs[j, k];
                            eqs[i, k] = temp;
                        }
                    }
                }
                for (j = i - 1; j >= 0; j--)
                {
                    if (eqs[j, j] < 0.000001f && eqs[j, j] > -0.000001f)
                    {
                        a = eqs[i, i];
                        b = eqs[j, i];

                        if (Math.Abs(a) > 0.000001f || Math.Abs(b) > 0.000001f)
                        {
                            mag = (float)Math.Sqrt(a * a + b * b);
                            a /= mag;
                            b /= mag;

                            for (k = 0; k < 4; k++)
                            {
                                temp = a * eqs[i, k] + b * eqs[j, k];
                                eqs[j, k] = b * eqs[i, k] - a * eqs[j, k];
                                eqs[i, k] = temp;
                            }
                        }
                    }
                }
            }

        }

        /**
         * Uses a jacobi method to return the eigenvectors and eigenvalues 
         * of a 3x3 symmetric matrix.  Note: "a" will be destroyed in this
         * process.  "d" will contain the eigenvalues sorted in order of 
         * decreasing modulus and v will contain the corresponding eigenvectors.
         *
         * @param a the 3x3 symmetric matrix to calculate the eigensystem for
         * @param d the variable to hold the eigenvalues
         * @param v the variables to hold the eigenvectors
         */
        public static void jacobi(float[,] u, float[] d, float[,] v)
        {
            int j, iq, ip, i;
            float tresh, theta, tau, t, sm, s, h, g, c;
            float[] b = new float[3], z = new float[3];
            float[,] a = new float[3, 3];

            a[0, 0] = u[0, 0];
            a[0, 1] = u[0, 1];
            a[0, 2] = u[0, 2];
            a[1, 0] = u[1, 0];
            a[1, 1] = u[1, 1];
            a[1, 2] = u[1, 2];
            a[2, 0] = u[2, 0];
            a[2, 1] = u[2, 1];
            a[2, 2] = u[2, 2];

            for (ip = 0; ip < 3; ip++)
            {
                for (iq = 0; iq < 3; iq++)
                {
                    v[ip, iq] = 0.0f;
                }
                v[ip, ip] = 1.0f;
            }

            for (ip = 0; ip < 3; ip++)
            {
                b[ip] = a[ip, ip];
                d[ip] = b[ip];
                z[ip] = 0.0f;
            }

            for (i = 1; i <= 50; i++)
            {
                sm = 0.0f;
                for (ip = 0; ip < 2; ip++)
                {
                    for (iq = ip + 1; iq < 3; iq++)
                    {
                        sm += (float)Math.Abs(a[ip, iq]);
                    }
                }

                if (sm == 0.0f)
                {
                    // sort the stupid things and transpose
                    a[0, 0] = v[0, 0];
                    a[0, 1] = v[1, 0];
                    a[0, 2] = v[2, 0];
                    a[1, 0] = v[0, 1];
                    a[1, 1] = v[1, 1];
                    a[1, 2] = v[2, 1];
                    a[2, 0] = v[0, 2];
                    a[2, 1] = v[1, 2];
                    a[2, 2] = v[2, 2];

                    if (Math.Abs(d[0]) < Math.Abs(d[1]))
                    {
                        sm = d[0];
                        d[0] = d[1];
                        d[1] = sm;

                        sm = a[0, 0];
                        a[0, 0] = a[1, 0];
                        a[1, 0] = sm;
                        sm = a[0, 1];
                        a[0, 1] = a[1, 1];
                        a[1, 1] = sm;
                        sm = a[0, 2];
                        a[0, 2] = a[1, 2];
                        a[1, 2] = sm;
                    }
                    if (Math.Abs(d[1]) < Math.Abs(d[2]))
                    {
                        sm = d[1];
                        d[1] = d[2];
                        d[2] = sm;

                        sm = a[1, 0];
                        a[1, 0] = a[2, 0];
                        a[2, 0] = sm;
                        sm = a[1, 1];
                        a[1, 1] = a[2, 1];
                        a[2, 1] = sm;
                        sm = a[1, 2];
                        a[1, 2] = a[2, 2];
                        a[2, 2] = sm;
                    }
                    if (Math.Abs(d[0]) < Math.Abs(d[1]))
                    {
                        sm = d[0];
                        d[0] = d[1];
                        d[1] = sm;

                        sm = a[0, 0];
                        a[0, 0] = a[1, 0];
                        a[1, 0] = sm;
                        sm = a[0, 1];
                        a[0, 1] = a[1, 1];
                        a[1, 1] = sm;
                        sm = a[0, 2];
                        a[0, 2] = a[1, 2];
                        a[1, 2] = sm;
                    }

                    v[0, 0] = a[0, 0];
                    v[0, 1] = a[0, 1];
                    v[0, 2] = a[0, 2];
                    v[1, 0] = a[1, 0];
                    v[1, 1] = a[1, 1];
                    v[1, 2] = a[1, 2];
                    v[2, 0] = a[2, 0];
                    v[2, 1] = a[2, 1];
                    v[2, 2] = a[2, 2];

                    return;
                }

                if (i < 4)
                {
                    tresh = 0.2f * sm / 9;
                }
                else
                {
                    tresh = 0.0f;
                }

                for (ip = 0; ip < 2; ip++)
                {
                    for (iq = ip + 1; iq < 3; iq++)
                    {
                        g = 100.0f * (float)Math.Abs(a[ip, iq]);
                        if (i > 4 && (float)(Math.Abs(d[ip]) + g) == (float)Math.Abs(d[ip])
                            && (float)(Math.Abs(d[iq]) + g) == (float)Math.Abs(d[iq]))
                        {
                            a[ip, iq] = 0.0f;
                        }
                        else
                        {
                            if (Math.Abs(a[ip, iq]) > tresh)
                            {
                                h = d[iq] - d[ip];
                                if ((float)(Math.Abs(h) + g) == (float)Math.Abs(h))
                                {
                                    t = (a[ip, iq]) / h;
                                }
                                else
                                {
                                    theta = 0.5f * h / (a[ip, iq]);
                                    t = 1.0f / ((float)Math.Abs(theta) + (float)Math.Sqrt(1.0f + theta * theta));
                                    if (theta < 0.0f)
                                    {
                                        t = -1.0f * t;
                                    }
                                }

                                c = 1.0f / (float)Math.Sqrt(1 + t * t);
                                s = t * c;
                                tau = s / (1.0f + c);
                                h = t * a[ip, iq];
                                z[ip] -= h;
                                z[iq] += h;
                                d[ip] -= h;
                                d[iq] += h;
                                a[ip, iq] = 0.0f;
                                for (j = 0; j <= ip - 1; j++)
                                {
                                    //ROTATE ( a, j, ip, j, iq );
                                    g = a[j, ip]; h = a[j, iq]; a[j, ip] = g - s * (h + g * tau); a[j, iq] = h + s * (g - h * tau);
                                }
                                for (j = ip + 1; j <= iq - 1; j++)
                                {
                                    //ROTATE ( a, ip, j, j, iq );
                                    g = a[ip, j]; h = a[j, iq]; a[ip, j] = g - s * (h + g * tau); a[j, iq] = h + s * (g - h * tau);
                                }
                                for (j = iq + 1; j < 3; j++)
                                {
                                    //ROTATE ( a, ip, j, iq, j );
                                    g = a[ip, j]; h = a[iq, j]; a[ip, j] = g - s * (h + g * tau); a[iq, j] = h + s * (g - h * tau);
                                }
                                for (j = 0; j < 3; j++)
                                {
                                    //ROTATE ( v, j, ip, j, iq );
                                    g = v[j, ip]; h = v[j, iq]; v[j, ip] = g - s * (h + g * tau); v[j, iq] = h + s * (g - h * tau);
                                }
                            }
                        }
                    }
                }

                for (ip = 0; ip < 3; ip++)
                {
                    b[ip] += z[ip];
                    d[ip] = b[ip];
                    z[ip] = 0.0f;
                }
            }
            System.Diagnostics.Debug.WriteLine("too many iterations in jacobi");
            return;
        }

        public static int estimateRank(float[] a)
        {
            float[] w = new float[3];
            float[,] u = new float[3, 3];
            float[,] mat = new float[3, 3];
            int i;

            mat[0, 0] = a[0];
            mat[0, 1] = a[1];
            mat[0, 2] = a[2];
            mat[1, 1] = a[3];
            mat[1, 2] = a[4];
            mat[2, 2] = a[5];
            mat[1, 0] = a[1];
            mat[2, 0] = a[2];
            mat[2, 1] = a[4];

            jacobi(mat, w, u);

            if (w[0] == 0.0f)
            {
                return 0;
            }
            else
            {
                for (i = 1; i < 3; i++)
                {
                    if (w[i] < 0.1f)
                    {
                        return i;
                    }
                }

                return 3;
            }

        }

        /**
         * Inverts a 3x3 symmetric matrix by computing the pseudo-inverse.
         *
         * @param mat the matrix to invert
         * @param midpoint the point to minimize towards
         * @param rvalue the variable to store the pseudo-inverse in
         * @param w the place to store the inverse of the eigenvalues
         * @param u the place to store the eigenvectors
         */
        public static void matInverse(float[,] mat, float[] midpoint, float[,] rvalue, float[] w, float[,] u)
        {
            // there is an implicit assumption that mat is symmetric and real
            // U and V in the SVD will then be the same matrix whose rows are the eigenvectors of mat
            // W will just be the eigenvalues of mat
            //		float w [ 3 ];
            //		float u [ 3 , 3 ];
            int i;

            jacobi(mat, w, u);

            if (w[0] == 0.0f)
            {
                //			printf ( "error: largest eigenvalue is 0!\n" );
            }
            else
            {
                for (i = 1; i < 3; i++)
                {
                    if (w[i] < 0.001f) // / w [ 0 ] < TOLERANCE )
                    {
                        w[i] = 0;
                    }
                    else
                    {
                        w[i] = 1.0f / w[i];
                    }
                }
                w[0] = 1.0f / w[0];
            }

            rvalue[0, 0] = w[0] * u[0, 0] * u[0, 0] +
                            w[1] * u[1, 0] * u[1, 0] +
                            w[2] * u[2, 0] * u[2, 0];
            rvalue[0, 1] = w[0] * u[0, 0] * u[0, 1] +
                            w[1] * u[1, 0] * u[1, 1] +
                            w[2] * u[2, 0] * u[2, 1];
            rvalue[0, 2] = w[0] * u[0, 0] * u[0, 2] +
                            w[1] * u[1, 0] * u[1, 2] +
                            w[2] * u[2, 0] * u[2, 2];
            rvalue[1, 0] = w[0] * u[0, 1] * u[0, 0] +
                            w[1] * u[1, 1] * u[1, 0] +
                            w[2] * u[2, 1] * u[2, 0];
            rvalue[1, 1] = w[0] * u[0, 1] * u[0, 1] +
                            w[1] * u[1, 1] * u[1, 1] +
                            w[2] * u[2, 1] * u[2, 1];
            rvalue[1, 2] = w[0] * u[0, 1] * u[0, 2] +
                            w[1] * u[1, 1] * u[1, 2] +
                            w[2] * u[2, 1] * u[2, 2];
            rvalue[2, 0] = w[0] * u[0, 2] * u[0, 0] +
                            w[1] * u[1, 2] * u[1, 0] +
                            w[2] * u[2, 2] * u[2, 0];
            rvalue[2, 1] = w[0] * u[0, 2] * u[0, 1] +
                            w[1] * u[1, 2] * u[1, 1] +
                            w[2] * u[2, 2] * u[2, 1];
            rvalue[2, 2] = w[0] * u[0, 2] * u[0, 2] +
                            w[1] * u[1, 2] * u[1, 2] +
                            w[2] * u[2, 2] * u[2, 2];
        }

        /**
         * Calculates the L2 norm of the residual (the error)
         * (Transpose[A].A).x = Transpose[A].B
         * 
         * @param a the matrix Transpose[A].A
         * @param b the matrix Transpose[A].B
         * @param btb the value Transpose[B].B
         * @param point the minimizer found
         *
         * @return the error of the minimizer
         */
        public static float calcError(float[,] a, float[] b, float btb, float[] point)
        {
            float rvalue = btb;

            rvalue += -2.0f * (point[0] * b[0] + point[1] * b[1] + point[2] * b[2]);
            rvalue += point[0] * (a[0, 0] * point[0] + a[0, 1] * point[1] + a[0, 2] * point[2]);
            rvalue += point[1] * (a[1, 0] * point[0] + a[1, 1] * point[1] + a[1, 2] * point[2]);
            rvalue += point[2] * (a[2, 0] * point[0] + a[2, 1] * point[1] + a[2, 2] * point[2]);

            return rvalue;
        }

        /**
         * Calculates the normal.  This function is not called and doesn't do
         * anything right now.  It was originally meant to orient the normals 
         * correctly that came from the principle eigenvector, but we're flat
         * shading now.
         */
        public static float[] calcNormal(float[] halfA, float[] norm, float[] expectedNorm)
        {
            /*
                float a [ 3 , 3 ];
                float w [ 3 ];
                float u [ 3 , 3 ];

                a [ 0 , 0 ] = halfA [ 0 ];
                a [ 0 , 1 ] = halfA [ 1 ];
                a [ 0 , 2 ] = halfA [ 2 ];
                a [ 1 , 1 ] = halfA [ 3 ];
                a [ 1 , 2 ] = halfA [ 4 ];
                a [ 1 , 0 ] = halfA [ 1 ];
                a [ 2 , 0 ] = halfA [ 2 ];
                a [ 2 , 1 ] = halfA [ 4 ];
                a [ 2 , 2 ] = halfA [ 5 ];

                jacobi ( a, w, u );

                if ( u [ 1 ] != 0 )
                {
                    if ( w [ 1 ] / w [ 0 ] > 0.2f )
                    {
                        // two dominant eigen values, just return the expectedNorm
                        norm [ 0 ] = expectedNorm [ 0 ];
                        norm [ 1 ] = expectedNorm [ 1 ];
                        norm [ 2 ] = expectedNorm [ 2 ];
                        return;
                    }
                }

                norm [ 0 ] = u [ 0 , 0 ];
                norm [ 1 ] = u [ 0 , 1 ];
                norm [ 2 ] = u [ 0 , 2 ];
            */
            float dot = norm[0] * expectedNorm[0] + norm[1] * expectedNorm[1] +
                        norm[2] * expectedNorm[2];

            if (dot < 0)
            {
                norm[0] *= -1.0f;
                norm[1] *= -1.0f;
                norm[2] *= -1.0f;

                dot *= -1.0f;
            }

            if (dot < 0.707f)
            {
                return expectedNorm;
            }
            else
            {
                return norm;
            }
        }

        public static void descent(float[,] A, float[] B, float[] guess, float[] bbstart, float[]bbend )
        {
            int i;
            float[] r = new float[3];
            float delta, delta0;
            int n = 10;
            float alpha, div;
            float[] newPoint = new float[3];
            float c;
            float[] store = new float[3];

            store[0] = guess[0];
            store[1] = guess[1];
            store[2] = guess[2];

            if (method == 2 || method == 0)
            {

                i = 0;
                r[0] = B[0] - (A[0, 0] * guess[0] + A[0, 1] * guess[1] + A[0, 2] * guess[2]);
                r[1] = B[1] - (A[1, 0] * guess[0] + A[1, 1] * guess[1] + A[1, 2] * guess[2]);
                r[2] = B[2] - (A[2, 0] * guess[0] + A[2, 1] * guess[1] + A[2, 2] * guess[2]);

                delta = r[0] * r[0] + r[1] * r[1] + r[2] * r[2];
                delta0 = delta * TOLERANCE * TOLERANCE;

                while (i < n && delta > delta0)
                {
                    div = r[0] * (A[0, 0] * r[0] + A[0, 1] * r[1] + A[0, 2] * r[2]);
                    div += r[1] * (A[1, 0] * r[0] + A[1, 1] * r[1] + A[1, 2] * r[2]);
                    div += r[2] * (A[2, 0] * r[0] + A[2, 1] * r[1] + A[2, 2] * r[2]);

                    if (Math.Abs(div) < 0.0000001f)
                    {
                        break;
                    }

                    alpha = delta / div;

                    newPoint[0] = guess[0] + alpha * r[0];
                    newPoint[1] = guess[1] + alpha * r[1];
                    newPoint[2] = guess[2] + alpha * r[2];

                    guess[0] = newPoint[0];
                    guess[1] = newPoint[1];
                    guess[2] = newPoint[2];

                    r[0] = B[0] - (A[0, 0] * guess[0] + A[0, 1] * guess[1] + A[0, 2] * guess[2]);
                    r[1] = B[1] - (A[1, 0] * guess[0] + A[1, 1] * guess[1] + A[1, 2] * guess[2]);
                    r[2] = B[2] - (A[2, 0] * guess[0] + A[2, 1] * guess[1] + A[2, 2] * guess[2]);

                    delta = r[0] * r[0] + r[1] * r[1] + r[2] * r[2];

                    i++;
                }

                if (guess[0] >= bbstart[0] && guess[0] <= bbend[0] &&
                    guess[1] >= bbstart[1] && guess[1] <= bbend[1] &&
                    guess[2] >= bbstart[2] && guess[2] <= bbend[2])
                {
                    return;
                }
            }

            if (method == 0 || method == 1)
            {
                c = A[0, 0] + A[1, 1] + A[2, 2];
                if (c == 0)
                {
                    return;
                }
                c = (0.75f / c);

                guess[0] = store[0];
                guess[1] = store[1];
                guess[2] = store[2];

                r[0] = B[0] - (A[0, 0] * guess[0] + A[0, 1] * guess[1] + A[0, 2] * guess[2]);
                r[1] = B[1] - (A[1, 0] * guess[0] + A[1, 1] * guess[1] + A[1, 2] * guess[2]);
                r[2] = B[2] - (A[2, 0] * guess[0] + A[2, 1] * guess[1] + A[2, 2] * guess[2]);

                for (i = 0; i < n; i++)
                {
                    guess[0] = guess[0] + c * r[0];
                    guess[1] = guess[1] + c * r[1];
                    guess[2] = guess[2] + c * r[2];

                    r[0] = B[0] - (A[0, 0] * guess[0] + A[0, 1] * guess[1] + A[0, 2] * guess[2]);
                    r[1] = B[1] - (A[1, 0] * guess[0] + A[1, 1] * guess[1] + A[1, 2] * guess[2]);
                    r[2] = B[2] - (A[2, 0] * guess[0] + A[2, 1] * guess[1] + A[2, 2] * guess[2]);
                }
            }
            /*
                if ( guess [ 0 ] > store [ 0 ] + 1 || guess [ 0 ] < store [ 0 ] - 1 ||
                    guess [ 1 ] > store [ 1 ] + 1 || guess [ 1 ] < store [ 1 ] - 1 ||
                    guess [ 2 ] > store [ 2 ] + 1 || guess [ 2 ] < store [ 2 ] - 1 )
                {
                    printf ( "water let point go from %f,%f,%f to %f,%f,%f\n", 
                        store [ 0 ], store [ 1 ], store [ 2 ], guess [ 0 ], guess [ 1 ], guess [ 2 ] );
                    printf ( "A is %f,%f,%f %f,%f,%f %f,%f,%f\n", A [ 0 , 0 ], A [ 0 , 1 ], A [ 0 , 2 ],
                        A [ 1 , 0 ] , A [ 1 , 1 ], A [ 1 , 2 ], A [ 2 , 0 ], A [ 2 , 1 ], A [ 2 , 2 ] );
                    printf ( "B is %f,%f,%f\n", B [ 0 ], B [ 1 ], B [ 2 ] );
                    printf ( "bounding box is %f,%f,%f to %f,%f,%f\n", 
                        box.begin.x, box.begin.y, box.begin.z, box.end.x, box.end.y, box.end.z );
                }
            */
        }

        /**
         * Calculates the minimizer of the given system and returns its error.
         *
         * @param halfA the compressed form of the symmetric matrix Transpose[A].A
         * @param b the matrix Transpose[A].B
         * @param btb the value Transpose[B].B
         * @param midpoint the point to minimize towards
         * @param rvalue the place to store the minimizer
         * @param box the volume bounding the voxel this QEF is for
         *
         * @return the error of the minimizer
         */
        public static float calcPoint(float[] halfA, float[] b, float btb, float[] midpoint, float[] rvalue, float[] bbstart, float[] bbend, float[] mat)
        {
            float[] newB = new float[3];
            float[,] a = new float[3, 3];
            float[,] inv = new float[3, 3];
            float[] w = new float[3];
            float[,] u = new float[3, 3];

            a[0, 0] = halfA[0];
            a[0, 1] = halfA[1];
            a[0, 2] = halfA[2];
            a[1, 1] = halfA[3];
            a[1, 2] = halfA[4];
            a[1, 0] = halfA[1];
            a[2, 0] = halfA[2];
            a[2, 1] = halfA[4];
            a[2, 2] = halfA[5];

            switch (method)
            {
                case 0:
                case 1:
                case 2:
                    rvalue[0] = midpoint[0];
                    rvalue[1] = midpoint[1];
                    rvalue[2] = midpoint[2];

                    descent(a, b, rvalue, bbstart,bbend);
                    return calcError(a, b, btb, rvalue);
                case 3:
                    matInverse(a, midpoint, inv, w, u);


                    newB[0] = b[0] - a[0, 0] * midpoint[0] - a[0, 1] * midpoint[1] - a[0, 2] * midpoint[2];
                    newB[1] = b[1] - a[1, 0] * midpoint[0] - a[1, 1] * midpoint[1] - a[1, 2] * midpoint[2];
                    newB[2] = b[2] - a[2, 0] * midpoint[0] - a[2, 1] * midpoint[1] - a[2, 2] * midpoint[2];

                    rvalue[0] = inv[0, 0] * newB[0] + inv[1, 0] * newB[1] + inv[2, 0] * newB[2] + midpoint[0];
                    rvalue[1] = inv[0, 1] * newB[0] + inv[1, 1] * newB[1] + inv[2, 1] * newB[2] + midpoint[1];
                    rvalue[2] = inv[0, 2] * newB[0] + inv[1, 2] * newB[1] + inv[2, 2] * newB[2] + midpoint[2];
                    return calcError(a, b, btb, rvalue);
                case 4:
                    method = 3;
                    calcPoint(halfA, b, btb, midpoint, rvalue, bbstart,bbend, mat);
                    method = 4;
                    /*
                            int rank;
                            float eqs [ 4 , 4 ];

                            // form the square matrix
                            eqs [ 0 , 0 ] = mat [ 0 ];
                            eqs [ 0 , 1 ] = mat [ 1 ];
                            eqs [ 0 , 2 ] = mat [ 2 ];
                            eqs [ 0 , 3 ] = mat [ 3 ];
                            eqs [ 1 , 1 ] = mat [ 4 ];
                            eqs [ 1 , 2 ] = mat [ 5 ];
                            eqs [ 1 , 3 ] = mat [ 6 ];
                            eqs [ 2 , 2 ] = mat [ 7 ];
                            eqs [ 2 , 3 ] = mat [ 8 ];
                            eqs [ 3 , 3 ] = mat [ 9 ];
                            eqs [ 1 , 0 ] = eqs [ 2 , 0 ] = eqs [ 2 , 1 ] = eqs [ 3 , 0 ] = eqs [ 3 , 1 ] = eqs [ 3 , 2 ] = 0;

                            // compute the new QR decomposition and rank
                            rank = qr ( eqs );

                            method = 2;
                            calcPoint ( halfA, b, btb, midpoint, rvalue, box, mat );
                            method = 4;
                    /*
                            if ( rank == 0 )
                            {
                                // it's zero, no equations
                                rvalue [ 0 ] = midpoint [ 0 ];
                                rvalue [ 1 ] = midpoint [ 1 ];
                                rvalue [ 2 ] = midpoint [ 2 ];
                            }
                            else 
                            {
                                if ( rank == 1 )
                                {
                                    // one equation, it's a plane
                                    float temp = ( eqs [ 0 , 0 ] * midpoint [ 0 ] + eqs [ 0 , 1 ] * midpoint [ 1 ] + eqs [ 0 , 2 ] * midpoint [ 2 ] - eqs [ 0 , 3 ] ) /
                                                ( eqs [ 0 , 0 ] * eqs [ 0 , 0 ] + eqs [ 0 , 1 ] * eqs [ 0 , 1 ] + eqs [ 0 , 2 ] * eqs [ 0 , 2 ] );

                                    rvalue [ 0 ] = midpoint [ 0 ] - temp * eqs [ 0 , 0 ];
                                    rvalue [ 1 ] = midpoint [ 1 ] - temp * eqs [ 0 , 1 ];
                                    rvalue [ 2 ] = midpoint [ 2 ] - temp * eqs [ 0 , 2 ];
                                }
                                else
                                {
                                    if ( rank == 2 )
                                    {
                                        // two equations, it's a line
                                        float a, b, c, d, e, f, g;

                                        // reduce back to upper triangular
                                        qr ( eqs, 2, 0.000001f );

                                        a = eqs [ 0 , 0 ];
                                        b = eqs [ 0 , 1 ];
                                        c = eqs [ 0 , 2 ];
                                        d = eqs [ 0 , 3 ];
                                        e = eqs [ 1 , 1 ];
                                        f = eqs [ 1 , 2 ];
                                        g = eqs [ 1 , 3 ];

                                        // solved using the equations
                                        // ax + by + cz = d
                                        //      ey + fz = g
                                        // minimize (x-px)^2 + (y-py)^2 + (z-pz)^2
                                        if ( a > 0.000001f || a < -0.000001f )
                                        {
                                            if ( e > 0.00000f || e < -0.000001f )
                                            {
                                                rvalue [ 2 ] = ( -1 * b * d * e * f + ( a * a + b * b ) * g * f + c * e * ( d * e - b * g ) +
                                                    a * e * ( ( b * f - c * e ) * midpoint [ 0 ] - a * f * midpoint [ 1 ] + a * e * midpoint [ 2 ] ) ) /
                                                    ( a * a * ( e * e + f * f ) + ( c * e - b * f ) * ( c * e - b * f ) );
                                                rvalue [ 1 ] = ( g - f * rvalue [ 2 ] ) / e;
                                                rvalue [ 0 ] = ( d - b * rvalue [ 1 ] - c * rvalue [ 2 ] ) / a;
                                            }
                                            else
                                            {
                                                // slightly degenerate case where e==0
                                                rvalue [ 2 ] = g / f;
                                                rvalue [ 1 ] = ( b * d * f - b * c * g - a * b * f * midpoint [ 0 ] + a * a * f * midpoint [ 1 ] ) /
                                                    ( a * a * f + b * b * f );
                                                rvalue [ 0 ] = ( d - b * rvalue [ 1 ] - c * rvalue [ 2 ] ) / a;
                                            }
                                        }
                                        else
                                        {
                                            // degenerate case where a==0 so e == 0 (upper triangular)

                                            rvalue [ 2 ] = g / f;
                                            rvalue [ 1 ] = ( d - c * rvalue [ 2 ] ) / b;
                                            rvalue [ 0 ] = midpoint [ 0 ];
                                        }

                                    }
                                    else
                                    {
                                        // must be three equations or more now... solve using back-substitution
                                        rvalue [ 2 ] = mat [ 8 ] / mat [ 7 ];
                                        rvalue [ 1 ] = ( mat [ 6 ] - mat [ 5 ] * rvalue [ 2 ] ) / mat [ 4 ];
                                        rvalue [ 0 ] = ( mat [ 3 ] - mat [ 2 ] * rvalue [ 2 ] - mat [ 1 ] * rvalue [ 1 ] ) / mat [ 0 ];
                                    }
                                }
                            }
                    */

                    float ret;
                    float tmp;

                    ret = mat[9] * mat[9];

                    tmp = mat[0] * rvalue[0] + mat[1] * rvalue[1] + mat[2] * rvalue[2] - mat[3];
                    ret += tmp * tmp;

                    tmp = mat[4] * rvalue[1] + mat[5] * rvalue[2] - mat[6];
                    ret += tmp * tmp;

                    tmp = mat[7] * rvalue[2] - mat[8];
                    ret += tmp * tmp;

                    return ret;
                case 5:
                    rvalue[0] = midpoint[0];
                    rvalue[1] = midpoint[1];
                    rvalue[2] = midpoint[2];

                    return calcError(a, b, btb, rvalue);
            }

            return 0;
        }

    }
}
